#include "bouncing_ball_simulation.h"
#include <stdio.h>
#include <time.h>

#include <base\cuda_device.cuh>
#include <collision\narrow_phase.h>
#include <physics\dynamics_kernel.cuh>

namespace gc {
	BouncingBallSimulation::BouncingBallSimulation(SimpleBox &sceneBox)
		:n_prims(0), dt(0.1),frame(0), d_balls(nullptr),d_rigids(nullptr)
	{
		this->sceneBox = sceneBox;
		lx = sceneBox.halfSize.x * 2;
		ly = sceneBox.halfSize.y * 2;
		lz = sceneBox.halfSize.z * 2;
		base.x = sceneBox.center.x - sceneBox.halfSize.x;
		base.y = sceneBox.center.y - sceneBox.halfSize.y;
		base.z = sceneBox.center.z - sceneBox.halfSize.z;

		d_cp.resize(10000000);

		MCudaDevice& CudaDevice = MCudaDevice::getInstance();
		CudaDevice.registerKernel(updateDynamics);
		CudaDevice.registerKernel(narrowPhase<SimpleSphere>);
	}

	BouncingBallSimulation::~BouncingBallSimulation() {
		if (d_balls) checkCudaErrors(cudaFree(d_balls));
		if (d_rigids) checkCudaErrors(cudaFree(d_rigids));
	}

	void BouncingBallSimulation::setTimePerFrame(Float dt) { this->dt = dt; }

	void BouncingBallSimulation::generateBalls(int number) {
		int sqrtn = int(sqrt(number));
		int nx = sqrtn + 2, nz = sqrtn + 2;
		n_prims = sqrtn * sqrtn;

		Float gridX = lx / nx;
		Float gridZ = lz / nz;

		Float radiusL = fmin(gridX, gridZ)*0.2;
		Float radiusR = radiusL * 2.5;

		Float posyL = sceneBox.center.y;
		Float posyR = sceneBox.center.y + sceneBox.halfSize.y - radiusR;

		Float velocityL = 0, velocityR = 5;
		Float mass = 5;
		auto material = MPhys::PhysicalMaterial::Iron;

		// one in every grid
		std::vector<SimpleSphere> balls(n_prims);
		std::vector<MPhys::RigidBody> rigids(n_prims);

		srand(time(0));
		int i = 0;
		for (Float x = base.x + gridX; x < base.x + lx - 2 * gridX; x += gridX) {
			for (Float z = base.z + gridZ; z < base.z + lz - 2 * gridZ; z += gridZ) {
				Float radius = genRand(radiusL, radiusR);
				Float posx = genRand(x, x + gridX);
				Float posy = genRand(posyL, posyR);
				Float posz = genRand(z, z + gridZ);
				Point3f pos{ posx,posy,posz };

				Float speed = genRand(velocityL, velocityR);
				Point3f v = uniformSampleDirection()*speed;

				balls[i] = SimpleSphere(pos, radius);
				rigids[i] = MPhys::RigidBody(mass, material);
				rigids[i].v = v;
				rigids[i].position = pos;
				++i;
			}
		}

		checkCudaErrors(GPUMalloc(d_balls, sizeof(SimpleSphere)*n_prims));
		checkCudaErrors(GPUMalloc(d_rigids, sizeof(MPhys::RigidBody)*n_prims));
		checkCudaErrors(CPU2GPU(d_balls, balls.data(), sizeof(SimpleSphere)*n_prims));
		checkCudaErrors(CPU2GPU(d_rigids, rigids.data(), sizeof(MPhys::RigidBody)*n_prims));

		lbvh.loadPrimitives(d_balls, n_prims);
	}

	void BouncingBallSimulation::produceFrame() {
		/*
		1. move every ball based on Newton physics
		2. pull back balls stuck in the wall
		3. build LBVH and find collision pairs
		4. do narrow phase interaction among CPs
		*/
		MCudaDevice& CudaDevice = MCudaDevice::getInstance();
		Point3f boxMax{ base.x + lx,base.y + ly,base.z + lz };
		CudaDevice.launchKernel(n_prims, 0, updateDynamics, n_prims, d_balls, d_rigids, dt, base, boxMax);
		lbvh.build();
		int CPsize = 0, capacity = d_cp.size();
		lbvh.broadPhaseCollison(getRawPtr(d_cp), &CPsize, &capacity);
		fprintf(stderr, "frame %d: BP collisions = %d\n", CPsize);
		CudaDevice.launchKernel(CPsize, 0, narrowPhase<SimpleSphere>, CPsize, getRawPtr(d_cp), d_balls, d_rigids);
	}

}